#!/bin/python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray, String
import re


class FloorPublisher(Node):
    def __init__(self):
        super().__init__('floor_publisher')
        
        # Create publisher for floor array
        self.floor_publisher_ = self.create_publisher(
            Int32MultiArray, 
            '/floor_arr', 
            10
        )
        
        # Create subscriber for text input (optional)
        self.text_subscriber_ = self.create_subscription(
            String,
            '/text_input',
            self.text_callback,
            10
        )
        
        # Parameters for floor range
        self.declare_parameter('min_floor', 1)
        self.declare_parameter('max_floor', 5)
        
        self.min_floor = self.get_parameter('min_floor').get_parameter_value().integer_value
        self.max_floor = self.get_parameter('max_floor').get_parameter_value().integer_value
        
        self.get_logger().info(f'Floor publisher node started (range: {self.min_floor}-{self.max_floor})')
        
        # Timer for periodic publishing (demonstration)
        self.timer = self.create_timer(5.0, self.timer_callback)
        self.demo_texts = [
            "我们一起去二楼和三楼",
            "去三楼",
            "一楼、二楼、五楼",
            "请到四楼会议室"
        ]
        self.demo_index = 0
    
    def parse_floors(self, s):
        """
        Parse floor string and extract all floor numbers within valid range.
        
        Args:
            s (str): Input string to parse
            
        Returns:
            list: List of floor numbers
        """
        # Preprocessing
        s = (s.replace('地下一', '地下1')
              .replace('地下二', '地下2')
              .replace('地下三', '地下3')
              .replace('负一', '-1')
              .replace('负二', '-2')
              .replace('负三', '-3'))
        
        chinese_to_digit = {
            '一': 1, '二': 2, '两': 2, '三': 3, '叁': 3,
            '四': 4, '五': 5, '六': 6, '七': 7, '八': 8, '九': 9,
            '十': 10, '零': 0
        }
        
        result = []
        
        # Find numeric patterns
        numbers = re.findall(r'[负\-]?\d+', s)
        
        for num_str in numbers:
            if num_str.startswith('负') or num_str.startswith('-'):
                floor_num = -int(num_str[1:])
            else:
                floor_num = int(num_str)
            
            if self.min_floor <= floor_num <= self.max_floor:
                result.append(floor_num)
        
        # If no numeric patterns found, look for Chinese characters near floor keywords
        if not result:
            floor_keywords = ['楼', '层']
            for i, char in enumerate(s):
                if char in chinese_to_digit:
                    is_near_keyword = False
                    
                    # Look ahead for keywords
                    lookahead = min(3, len(s) - i - 1)
                    for j in range(1, lookahead + 1):
                        if s[i + j] in floor_keywords:
                            is_near_keyword = True
                            break
                    
                    # Look behind for keywords
                    if not is_near_keyword and i > 0:
                        lookbehind = min(2, i)
                        for j in range(1, lookbehind + 1):
                            if s[i - j] in floor_keywords:
                                is_near_keyword = True
                                break
                    
                    if is_near_keyword:
                        floor_num = chinese_to_digit[char]
                        if self.min_floor <= floor_num <= self.max_floor and floor_num not in result:
                            result.append(floor_num)
        
        # Sort and remove duplicates
        result = sorted(list(set(result)))
        
        # Return -99 if no valid floors found
        return result if result else [-99]
    
    def publish_floors(self, floor_array):
        """
        Publish floor array to ROS2 topic.
        
        Args:
            floor_array (list): List of floor numbers
        """
        msg = Int32MultiArray()
        msg.data = floor_array
        
        self.floor_publisher_.publish(msg)
        self.get_logger().info(f'Published floors: {floor_array}')
    
    def text_callback(self, msg):
        """
        Callback for text input subscription.
        
        Args:
            msg (String): Text message
        """
        text = msg.data
        floors = self.parse_floors(text)
        self.get_logger().info(f"Parsed '{text}' -> {floors}")
        self.publish_floors(floors)
    
    def timer_callback(self):
        """
        Timer callback for demonstration purposes.
        """
        if self.demo_index < len(self.demo_texts):
            demo_text = self.demo_texts[self.demo_index]
            floors = self.parse_floors(demo_text)
            self.get_logger().info(f"Demo: Parsed '{demo_text}' -> {floors}")
            self.publish_floors(floors)
            self.demo_index += 1
        else:
            # Stop demo after all texts are processed
            self.timer.cancel()
            self.get_logger().info("Demo completed. Node continues to listen for /text_input messages.")
    
    def process_text_and_publish(self, text):
        """
        Public method to process text and publish floors.
        
        Args:
            text (str): Text to parse
            
        Returns:
            list: Parsed floor numbers
        """
        floors = self.parse_floors(text)
        self.publish_floors(floors)
        return floors


def main(args=None):
    rclpy.init(args=args)
    
    floor_publisher = FloorPublisher()
    
    try:
        rclpy.spin(floor_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        floor_publisher.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()